/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "Protocol.h"

TFSRSimObjectRepositionCmd::TFSRSimObjectRepositionCmd()
  :/*Rotation({1,0,0,
             0,1,0,
             0,0,1}),*/
  x(0),
  y(0),
  z(0),
  heading(0)
{
  /*
  Rotation[0]=1;
  Rotation[1]=0;
  Rotation[2]=0;
  Rotation[3]=0;
  Rotation[4]=1;
  Rotation[5]=0;
  Rotation[6]=0;
  Rotation[7]=0;
  Rotation[8]=1;
  */
}
  
const char* TFSRSimObjectRepositionCmd::createData(int &size)
{
  binbag.Clear();
  
  //binbag.AddBinary(0,(char*)&Rotation,9*sizeof(float));
  binbag.AddBinary(0,(char*)&heading,sizeof(float));
  binbag.AddBinary(0,(char*)&x,sizeof(float));
  binbag.AddBinary(0,(char*)&y,sizeof(float));
  binbag.AddBinary(0,(char*)&z,sizeof(float));
  
  size=binbag.GetBufferSize();
  
  return binbag.GetBuffer();
  
}

bool TFSRSimObjectRepositionCmd::parseData(char *data, int size)
{
  if (binbag.Decode((char*)data,size))
  {
    const gim::binbag::TBinBlob *blob;
    blob=binbag.GetNextBinary();
    memcpy(&heading,blob->data,blob->size);
    /*if (blob->size!=9*sizeof(float))
    {
      dPrint(1,"Rotation data size doesn't match!")
      return false;
    }
    memcpy(&Rotation,blob->data,blob->size);
    */
    blob=binbag.GetNextBinary();
    memcpy(&x,blob->data,blob->size);
    blob=binbag.GetNextBinary();
    memcpy(&y,blob->data,blob->size);
    blob=binbag.GetNextBinary();
    memcpy(&z,blob->data,blob->size);
    return true;
  }
  return false;
}
